#include "defines.h"
#include "elf.h"
#include "lib.h"
#include "xmodem.h"

struct elf_header {
    struct {
        unsigned char magic[4];
        unsigned char class;
        unsigned char format;
        unsigned char version;
        unsigned char abi;
        unsigned char abi_version;
        unsigned char reserve[7];
    } id;
    short type;
    short arch;
    long version;
    long entry_point;
    long program_header_offset;
    long section_header_offset;
    long flags;
    short header_size;
    short program_header_size;
    short program_header_num;
    short section_header_size;
    short section_header_num;
    short section_name_index;    
};

struct elf_program_header {
    long type;
    long offset;
    long virtual_addr;
    long physical_addr;
    long file_size;
    long memory_size;
    long flags;
    long align;
};

static int elf_check(struct elf_header *header)
{
    if (memcmp(header->id.magic, "\x7f" "ELF", 4))
        return -1;
        
    if (header->id.class != 1) return -1;
    if (header->id.format != 2) return -1;
    if (header->id.version != 1) return -1;
    if (header->type != 2) return -1;
    if (header->version != 1) return -1;
    if ((header->arch != 46) && (header->arch != 47)) return -1;
    
    return 0;
}

static int elf_load_program(struct elf_header *header)
{
    int i;
    struct elf_program_header *phdr;
    
    for (i = 0; i < header->program_header_num; i++) {
        phdr = (struct elf_program_header *) ((char *)header + header->program_header_offset + header->program_header_size * i);
        if (phdr->type != 1)
            continue;
        
        memcpy((char *)phdr->physical_addr, (char *)header + phdr->offset, phdr->file_size);
        memset((char *)phdr->physical_addr + phdr->file_size, 0, phdr->memory_size - phdr->file_size);
    }
    
    return 0;
}

char *elf_load(char *buf)
{
    struct elf_header *header = (struct elf_header *) buf;
    
    if (elf_check(header) < 0)
        return NULL;
        
    if (elf_load_program(header) < 0)
        return NULL;
        
    return (char *)header->entry_point;
}

char *elf_boot(char *blockbuf, char *headerbuf)
{
    long offset = 0;
    int i, r;
    struct elf_header *ehdr = (struct elf_header *) headerbuf;
    unsigned char block_number = 1;
    struct elf_program_header *phdr;
    char *p;

    r = xmodem_recv_block(blockbuf, block_number++, 0);
    if (r <= 0)
        return NULL;
    memcpy(headerbuf, blockbuf, r);
    offset += r;
    if (elf_check(ehdr) < 0) {
        xmodem_can();
        return NULL;
    }
    
    while (offset < ehdr->program_header_offset + ehdr->program_header_size * ehdr->program_header_num) {
        r = xmodem_recv_block(blockbuf, block_number++, 1);
        if (r <= 0)
            return NULL;     
        memcpy(headerbuf + offset, blockbuf, r);
        offset += r;
    }

    for (i = 0; i < ehdr->program_header_num; i++) {
        phdr = (struct elf_program_header *) (headerbuf + ehdr->program_header_offset + ehdr->program_header_size * i);       
        if (phdr->type != 1)
            continue;
        p = (char *)phdr->physical_addr;

        if (phdr->file_size) {
            while (offset <= phdr->offset) {
                r = xmodem_recv_block(blockbuf, block_number++, 1);
                if (r <= 0)
                    return NULL;     
                offset += r;            
            }
            
            if (offset >= phdr->offset + phdr->file_size) {
                memcpy(p, blockbuf + r - (offset - phdr->offset), phdr->file_size);
                p += phdr->file_size;
                
            } else {
                memcpy(p, blockbuf + r - (offset - phdr->offset), offset - phdr->offset);
                p += offset - phdr->offset;
                
                while (1) {
                    r = xmodem_recv_block(blockbuf, block_number++, 1);
                    if (r <= 0)
                        return NULL;     
                    offset += r;            
                                    
                    if (offset >= phdr->offset + phdr->file_size)
                        break;
                    memcpy(p, blockbuf, r);
                    p += r;
                }
            
                memcpy(p, blockbuf, r - (offset - (phdr->offset + phdr->file_size)));
                p += r - (offset - (phdr->offset + phdr->file_size));
            }
        }
            
        memset(p, 0, phdr->memory_size - phdr->file_size);            
    }
    
    while (xmodem_recv_block(blockbuf, block_number++, 1) > 0)  /* 読み終える */
        ;
    
    return (char *)ehdr->entry_point;
}
